#include "Vector.h"
#include "Matrix.h"
											// ///////////////////
											// VEC2
											// /////////////////

/// Default two-dimentional constructor. All is set to 0.
template<class T> vec2<T>::vec2( )									: x(  0 ), y(  0 ) { }
/// Two-dimentional constant constructor. All is set to the parameter.
/**	@param xx - the value for x and y */
template<class T> vec2<T>::vec2( T const& xx )					: x( xx ), y( xx ) { }
/// Two-dimentional constructor. Provide the two values.
/**	@param xx - x value
		@param yy - y value */
template<class T> vec2<T>::vec2( const T &xx, const T &yy )	: x( xx ), y( yy ) { }
/// Two-dimentional copy constructor. Float cast.
/**	@param copy - 2d float vector to copy from */
template<class T> vec2<T>::vec2( const vec2<float> &copy )	: x( (T)copy.x ), y( (T)copy.y ) { }
/// Two-dimentional copy constructor. Double cast.
/**	@param copy - 2d double vector to copy from */
template<class T> vec2<T>::vec2( const vec2<double> &copy )	: x( (T)copy.x ), y( (T)copy.y ) { }
/// Two-dimentional copy constructor from three-dimentional vector. Float cast.
/**	@param copy - 3d float vector to copy x,y from */
template<class T> vec2<T>::vec2( const vec3<float> &copy )	: x( (T)copy.x ), y( (T)copy.y ) { }
/// Two-dimentional copy constructor from three-dimentional vector. Double cast.
/**	@param copy - 3d double vector to copy x,y from */
template<class T> vec2<T>::vec2( const vec3<double> &copy )	: x( (T)copy.x ), y( (T)copy.y ) { }

/// Test for equality for 2d vector
/**	@param v - vector to check if the same
		@return true if both vector are the same */
template<class T> bool 				vec2<T>::operator==( const vec2<T> &v ) {
	return ( x==v.x && y==v.y );
}
/// Test for inequality from 2d vector
/**	@param v - vector to check for inequality
		@return true if not equal */
template<class T> bool 				vec2<T>::operator!=( const vec2<T> &v ) {
	return ( x!=v.x || y!=v.y );
}
/// Self subtraction by 2d vector
/**	@param b - 2d vector to substract from this one */
template<class T> void 				vec2<T>::operator-=( const vec2<T> &b ) {
	x -= b.x; y -= b.y;
}
/// Self addition by 2d vector
/**	@param b - 2d vector to add to this one */
template<class T> void 				vec2<T>::operator+=( const vec2<T> &b ) {
	x += b.x; y += b.y;
}
/// Test for equality to 3d vector
/**	@param v - 3d vector to check if has the same x and y elements
		@return true if x,y of both vectors are the same */
template<class T> bool 				vec2<T>::operator==( const vec3<T> &v ) {
	return ( x == v.x && y == v.y );
}
/// Test for inequality to 3d vector
/**	@param v - 3d vector to check if has different x and y elements
		@return true if x,y of both vectors are different */
template<class T> bool 				vec2<T>::operator!=( const vec3<T> &v ) {
	return ( x!=v.x || y!=v.y );
}
/// Self subtraction by 3d vector
/**	@param b - 3d vector to substract x,y from this one */
template<class T> void 				vec2<T>::operator-=( const vec3<T> &b ) {
	x -= b.x; y -= b.y;
}
/// Self addition with 3d vector
/**	@param b - 3d vector to add x,y from this one */
template<class T> void 				vec2<T>::operator+=( const vec3<T> &b ) {
	x += b.x; y += b.y;
}
/// Self uniform scale (multiplicate)
/**	@param scalar - value to multiply to the vector elements */
template<class T> void 				vec2<T>::operator*=( const T &scalar ) {
	x *= scalar; y *= scalar;
}
/// Self uniform scale (divide)
/**	@param scalar - value to divide the vector elements
		@return this vector */
template<class T> vec2<T> 			vec2<T>::operator/=( const T &scalar ) {
	x /= scalar; y /= scalar;
	return *this;
}
/// Set to value (2d vector)
/**	@param v - 2d vector to copy from
		@return this vector */
template<class T> const vec2<T> 	vec2<T>::operator= ( const vec2<T> &v ) {
	x = v.x; y = v.y;
	return *this;
}
/// Set to value (3d vector)
/**	@param v - 3d vector to copy x,y from
		@return this vector */
template<class T> const	vec2<T> 	vec2<T>::operator= ( const vec3<T> &v ) {
	x = v.x; y = v.y;
	return *this;
}
/// Set to value (const)
/**	@param v - value to set x,y to
		@return this vector */
template<class T> const	vec2<T> 	vec2<T>::operator= ( const T &v ) {
	x = v; y = v;
	return *this;
}
/// Negation
/**	@return inversed vector */
template<class T> vec2<T> 			vec2<T>::operator- ( void ) 					const {
	return vec2<T>(-x,-y);
}
/// Subtraction
/**	@param b - 2d vector to subtract from this one
		@return new vector */
template<class T> vec2<T> 			vec2<T>::operator- ( const vec2<T> &b ) 	const {
	return vec2<T>( x - b.x, y - b.y );
}
/// Addition
/**	@param b - 2d vector to add to this one
		@return new vector */
template<class T> vec2<T> 			vec2<T>::operator+ ( const vec2<T> &b ) 	const {
	return vec2<T>( x + b.x, y + b.y );
}
/// Subtraction
/**	@param b - 3d vector to subtract from this one
		@return new 3d vector */
template<class T> vec3<T> 			vec2<T>::operator- ( const vec3<T> &b ) 	const {
	return vec3<T>( x - b.x, y - b.y, - b.z );
}
/// Addition
/**	@param b - 3d vector to add to this one
		@return new 3d vector */
template<class T> vec3<T> 			vec2<T>::operator+ ( const vec3<T> &b ) 	const {
	return vec3<T>( x + b.x, y + b.y, b.z );
}
/// Dot product
/**	@param dot - 2d vector to dot with this one
		@return the 1d dot product */
template<class T> T const 			vec2<T>::operator* ( const vec2<T> &dot ) const {
   return x*dot.x + y*dot.y;
}
/// Uniform scale (multiplicate)
/**	@param scalar - 1d scalar to multiply with this vector
		@return new 2d vector */
template<class T> vec2<T> 			vec2<T>::operator* ( const T &scalar ) 	const {
   return vec2<T>( x*scalar, y*scalar );
}
/// Uniform scale (divide)
/**	@param scalar - 1d scalar to devide this vector with
		@return new 2d vector */
template<class T> vec2<T> 			vec2<T>::operator/ ( const T &scalar ) 	const {
   return vec2<T>( x/scalar, y/scalar );
}
/// Angle between two vectors
/**	@param vector - second edge of the arc
		@return the angle between the two vectors */
template<class T> T 					vec2<T>::Angle( const vec2<T> &vector ) {
	return acos( *this * vector );
}
/// Relative angle between two vectors. May be negative and means 'vector' is blabla angles relatively to 'this' vector
/**	@param vector - second edge of the arc
		@return the relative angle between the two vectors */
template<class T> T 					vec2<T>::AngleRel( const vec2<T> &vector ) {
	return atan2( vector.y, vector.x ) - atan2( (*this).y, (*this).x );
}
/// Normalize self
/**	@return this vector, normalized */
template<class T> vec2<T> 			vec2<T>::N( ) {
	T t_length = Length( );
	x /= t_length;
	y /= t_length;
	return *this;
}
/// Return normalized
/**	@return new vector, normalized */
template<class T> vec2<T> 			vec2<T>::Normalized( ) {
	return (*this)/Length();
}
/// Get length
/**	@return the length of the vector */
template<class T> T const 			vec2<T>::Length( ) const {
	return (T)sqrt( *this * *this );
}
/// Get length squared
/**	@return the squared length of the vector */
template<class T> T const 			vec2<T>::Lengthsq( ) const {
	return (T)( *this * *this );
}
/// Set data
/**	@param xx - x element
		@param yy - y element */
template<class T> void 				vec2<T>::Set( T const &xx, T const &yy ) {
	x = xx; y = yy;
}


											// ///////////////////
											// VEC3
											// /////////////////

/// Vec2 Constructors:
template<class T> vec3<T>::vec3( )															: x(0),           y(0),           z(0)		{}
template<class T> vec3<T>::vec3( T const& xx, T const& yy, T const& zz )		: x( xx ),        y( yy ),        z( zz ) {}
template<class T> vec3<T>::vec3( const vec2<float> 	&copy, const T &zz )		: x( (T)copy.x ), y( (T)copy.y ), z( zz ) {}
template<class T> vec3<T>::vec3( const vec2<double> 	&copy, const T &zz )		: x( (T)copy.x ), y( (T)copy.y ), z( zz ) {}
template<class T> vec3<T>::vec3( const vec3<float> 	&copy )						: x( (T)copy.x ), y( (T)copy.y ), z( (T)copy.z ) {}
template<class T> vec3<T>::vec3( const vec3<double> 	&copy )						: x( (T)copy.x ), y( (T)copy.y ), z( (T)copy.z ) {}
template<class T> vec3<T>::vec3( const vec4<float> 	&copy )						: x( (T)copy.x ), y( (T)copy.y ), z( (T)copy.z ) {}
template<class T> vec3<T>::vec3( const vec4<double> 	&copy )						: x( (T)copy.x ), y( (T)copy.y ), z( (T)copy.z ) {}
template<class T> vec3<T>::vec3( T const& xx )											: x( xx ),        y( xx ),        z( xx ) {}
/// Test for equality
template<class T> bool		vec3<T>::operator==( const vec3<T> &v ) {
	return ( x==v.x && y==v.y && z==v.z );
}
/// Test for inequality
template<class T> bool		vec3<T>::operator!=( const vec3<T> &v ) {
	return ( x!=v.x || y!=v.y || z!=v.z );
}
/// Self subtraction
template<class T> void 		vec3<T>::operator-=( const vec3<T> &b ) {
	x -= b.x; y -= b.y; z -= b.z;
}
/// Self addition
template<class T> void 		vec3<T>::operator+=( const vec3<T> &b ) {
	x += b.x; y += b.y; z += b.z;
}
/// Test for equality
template<class T> bool		vec3<T>::operator==( const vec2<T> &v ) {
	return ( x==v.x && y==v.y && z==0 );
}
/// Test for inequality
template<class T> bool		vec3<T>::operator!=( const vec2<T> &v ) {
	return ( x!=v.x || y!=v.y || z!=0 );
}
/// Self subtraction
template<class T> void 		vec3<T>::operator-=( const vec2<T> &b ) {
	x -= b.x; y -= b.y;
}
/// Self addition
template<class T> void 		vec3<T>::operator+=( const vec2<T> &b ) {
	x += b.x; y += b.y;
}
/// Self uniform scale (multiplicate)
template<class T> void 		vec3<T>::operator*=( const T &scalar ) {
	x *= scalar; y *= scalar; z *= scalar;
}
/// Self uniform scale (divide)
template<class T> void 		vec3<T>::operator/=( const T &scalar ) {
	x /= scalar; y /= scalar; z /= scalar;
}
/// Set to value (vector)
template<class T> const 	vec3<T> vec3<T>::operator= ( const vec3<T> &v ) {
	x = v.x; y = v.y; z = v.z;
	return *this;
}
/// Set to value (const)
template<class T> const 	vec3<T> vec3<T>::operator= ( const T &v ) {
	x = v; y = v; z = v;
	return *this;
}
/// Negaton
template<class T> vec3<T>  vec3<T>::operator- ( void ) 						const {
	return vec3<T>(-x,-y,-z);
}
/// Subtraction
template<class T> vec3<T>  vec3<T>::operator- ( const vec3<T> &b )		const {
	return vec3<T>( x - b.x, y - b.y, z - b.z );
}
/// Addition
template<class T> vec3<T>  vec3<T>::operator+ ( const vec3<T> &b )		const {
	return vec3<T>( x + b.x, y + b.y, z + b.z );
}
/// Subtraction
template<class T> vec3<T>  vec3<T>::operator- ( const vec2<T> &b )		const {
	return vec3<T>( x - b.x, y - b.y, z );
}
/// Addition
template<class T> vec3<T>  vec3<T>::operator+ ( const vec2<T> &b )		const {
	return vec3<T>( x + b.x, y + b.y, z );
}
/// Dot product
template<class T> T const  vec3<T>::operator* ( const vec3<T> &dot )		const {
   return x*dot.x + y*dot.y + z*dot.z;
}
/// Uniform scale (multiplicate)
template<class T> vec3<T>  vec3<T>::operator* ( const double &scalar )	const {
   return vec3<T>( x*((T)scalar), y*((T)scalar), z*((T)scalar) );
}
/// Uniform scale (multiplicate)
template<class T> vec3<T>  vec3<T>::operator* ( const float &scalar )	const {
   return vec3<T>( x*((T)scalar), y*((T)scalar), z*((T)scalar) );
}
/// Uniform scale (divide)
template<class T> vec3<T>  vec3<T>::operator/ ( const T &scalar )			const {
   return vec3<T>( x/scalar, y/scalar, z/scalar );
}
/// Cross product
template<class T> vec3<T>  vec3<T>::operator% ( const vec3<T> &v )		const {
	return vec3<T>(
		y*v.z - z*v.y,
		z*v.x - x*v.z,
		x*v.y - y*v.x );
}
/// Angle between two vectors
template<class T> T      	vec3<T>::Angle( const vec3<T> &vector, vec3<T> &axis ) {
	axis = *this % vector;
	axis.N();
	return acos( *this * vector );
}
/// Normalize
template<class T> vec3<T>  vec3<T>::N( ) {
	T l = Length();
	x /= l;
	y /= l;
	z /= l;
	return *this;
}
/// Return normalized
template<class T> vec3<T>  vec3<T>::Normalized( ) {
	return *this/Length();
}
/// Get length
template<class T> T const	vec3<T>::Length( ) 									const {
	return (T)sqrt( *this * *this );
}
/// Get length squared
template<class T> T const	vec3<T>::Lengthsq( ) 								const {
	return (T)( *this * *this );
}
/// Set values
template<class T> void		vec3<T>::Set( T const &xx, T const &yy, T const &zz ) {
	x = xx; y = yy; z = zz;
}
/// Project into XY plane
template<class T> vec3<T>	vec3<T>::ProjXY( ) const {
	return vec3<T>( x, y, 0 );
}
/// Project into XZ plane
template<class T> vec3<T>	vec3<T>::ProjXZ( ) const {
	return vec3<T>( x, 0, z );
}
/// Project into YZ plane
template<class T> vec3<T>	vec3<T>::ProjYZ( ) const {
	return vec3<T>( 0, y, z );
}
/// Linear interpolation
template<class T> vec3<T>	vec3<T>::Linear( const vec3<T> &vec, const T &cur, const T &begin, const T &end ) {
	if( end <= begin) return vec3<T>(0);

	vec3<T> interpolator;
	T ratio = (end - begin) / (cur - begin);
	interpolator.x = (x + vec.x)*ratio;
	interpolator.y = (y + vec.y)*ratio;
	interpolator.z = (z + vec.z)*ratio;

	return *this + interpolator;
}
/// Rotate vector around arbitrary axis by angle in radians
template<class T> void		vec3<T>::Rotate( const vec3<T> &axe, const T &angle ) {
   T radian =  ( angle );
  	T fCos = cos( radian );
	T fSin = sin( radian );
	T fSum = (T)1.0 - fCos;

    mat4<T> rotation(
    ( axe.x*axe.x ) * fSum + fCos, ( axe.x*axe.y ) * fSum - ( axe.y*fSin ), ( axe.x*axe.z ) * fSum + ( axe.y*fSin ), 0,
    ( axe.y*axe.x ) * fSum + ( axe.y*fSin ), ( axe.y*axe.x ) * fSum + fCos, ( axe.x*axe.z ) * fSum - ( axe.x*fSin ), 0,
    ( axe.z*axe.x ) * fSum - ( axe.y*fSin ), ( axe.z*axe.x ) * fSum + ( axe.x*fSin ), ( axe.z*axe.z ) * fSum + fCos, 0,
    0, 0, 0, (T)1.0 );

	 vec3<T> buf = rotation * (*this);
	 x = buf.x; y = buf.y; z = buf.z;
}





											// ///////////////////
											// VEC4
											// /////////////////

/// Vec2 Constructors:
template<class T> vec4<T>::vec4( )																			: x(0),           y(0),           z(0),				w(0) {}
template<class T> vec4<T>::vec4( T const& xx, T const& yy, T const& zz )						: x( xx ),        y( yy ),        z( zz ),			w(0) {}
template<class T> vec4<T>::vec4( T const& xx, T const& yy, T const& zz, T const& ww )		: x( xx ),        y( yy ),        z( zz ),			w(ww) {}
template<class T> vec4<T>::vec4( const vec2<float> 	&copy, const vec2<float> &copy2 )	: x( (T)copy.x ), y( (T)copy.y ), z( (T)copy2.x ), w( (T)copy2.y ) {}
template<class T> vec4<T>::vec4( const vec2<double> 	&copy, const vec2<double> &copy2 )	: x( (T)copy.x ), y( (T)copy.y ), z( (T)copy2.x ), w( (T)copy2.y ) {}
template<class T> vec4<T>::vec4( const vec3<float> 	&copy )										: x( (T)copy.x ), y( (T)copy.y ), z( (T)copy.z ),	w(0) {}
template<class T> vec4<T>::vec4( const vec3<double> 	&copy )										: x( (T)copy.x ), y( (T)copy.y ), z( (T)copy.z ),  w(0) {}
template<class T> vec4<T>::vec4( const vec3<float> 	&copy, const T &ww )						: x( (T)copy.x ), y( (T)copy.y ), z( (T)copy.z ),	w(ww) {}
template<class T> vec4<T>::vec4( const vec3<double> 	&copy, const T &ww )						: x( (T)copy.x ), y( (T)copy.y ), z( (T)copy.z ),  w(ww) {}
template<class T> vec4<T>::vec4( T const& xx )															: x( xx ),        y( xx ),        z( xx ),			w( xx ) {}
template<class T> vec4<T>::vec4( const vec4<float> 	&copy )										: x( (T)copy.x ), y( (T)copy.y ), z( (T)copy.z ),	w( (T)copy.w ) {}
template<class T> vec4<T>::vec4( const vec4<double> 	&copy )										: x( (T)copy.x ), y( (T)copy.y ), z( (T)copy.z ),  w( (T)copy.w ) {}
/// Test for equality
template<class T> bool		vec4<T>::operator==( const vec4<T> &v ) {
	return ( x==v.x && y==v.y && z==v.z && w==v.w );
}
/// Test for inequality
template<class T> bool		vec4<T>::operator!=( const vec4<T> &v ) {
	return ( x!=v.x || y!=v.y || z!=v.z || w!=v.w );
}
/// Self subtraction
template<class T> void		vec4<T>::operator-=( const vec4<T> &b ) {
	x -= b.x; y -= b.y; z -= b.z; w -= b.w;
}
/// Self addition
template<class T> void		vec4<T>::operator+=( const vec4<T> &b ) {
	x += b.x; y += b.y; z += b.z; w += b.w;
}
/// Self uniform scale (multiplicate)
template<class T> void		vec4<T>::operator*=( const T &scalar ) {
	x *= scalar; y *= scalar; z *= scalar; w *= scalar;
}
/// Self uniform scale (divide)
template<class T> void		vec4<T>::operator/=( const T &scalar ) {
	x /= scalar; y /= scalar; z /= scalar; w /= scalar;
}
/// Set to value (vector)
template<class T> void		vec4<T>::operator= ( const vec4<T> &v ) {
	x = v.x; y = v.y; z = v.z; w = v.w;
}
/// Set to value (const)
template<class T> void		vec4<T>::operator= ( const T &v ) {
	x = v; y = v; z = v; w = v;
}
/// Negaton
template<class T> vec4<T>  vec4<T>::operator- ( void ) 					const {
	return vec4<T>(-x,-y,-z,-w);
}
/// Subtraction
template<class T> vec4<T>  vec4<T>::operator- ( const vec4<T> &b )	const {
	return vec4<T>( x - b.x, y - b.y, z - b.z, w - b.w );
}
/// Addition
template<class T> vec4<T>  vec4<T>::operator+ ( const vec4<T> &b )	const {
	return vec4<T>( x + b.x, y + b.y, z + b.z, w + b.w );
}
/// Dot product
template<class T> T const  vec4<T>::operator* ( const vec4<T> &dot)	const {
   return x*dot.x + y*dot.y + z*dot.z;
}
/// Uniform scale (multiplicate)
template<class T> vec4<T>  vec4<T>::operator* ( const T &scalar)		const {
   return vec4<T>( x*scalar, y*scalar, z*scalar, w*scalar );
}
/// Uniform scale (divide)
template<class T> vec4<T>  vec4<T>::operator/ ( const T &scalar)		const {
   return vec4<T>( x/scalar, y/scalar, z/scalar, w/scalar );
}
/// Cross product
template<class T> vec4<T>  vec4<T>::operator% ( const vec4<T> &v)		const {
	return vec4<T>(
		y*v.z - z*v.y,
		z*v.x - x*v.z,
		x*v.y - y*v.x, 0 );
}
/// Angle between two vectors
template<class T> T      	vec4<T>::Angle( const vec4<T> &vector, vec4<T> &axis ) {
	axis = *this % vector;
	axis.N();
	return acos( *this * vector );
}
/// Normalize
template<class T> vec4<T>  vec4<T>::N( ) {
	T l = Length();
	x /= l;
	y /= l;
	z /= l;
	w /= l;
	return *this;
}
/// Return normalized
template<class T> vec4<T>  vec4<T>::Normalized( ) {
	return *this/Length();
}
/// Get length
template<class T> T const	vec4<T>::Length( ) 	const {
	return (T)sqrt( *this * *this );
}
/// Get length squared
template<class T> T const	vec4<T>::Lengthsq( ) const {
	return (T)( *this * *this );
}
/// Set values (3)
template<class T> void		vec4<T>::Set( T const &xx, T const &yy, T const &zz ) {
	x = xx; y = yy; z = zz;
}
/// Set values (4)
template<class T> void		vec4<T>::Set( T const &xx, T const &yy, T const &zz, T const &ww ) {
	x = xx; y = yy; z = zz; w = ww;
}
/// Project into XY plane
template<class T> vec4<T>	vec4<T>::ProjXY( ) const {
	return vec4<T>( x, y, 0, 0 );
}
/// Project into XZ plane
template<class T> vec4<T>	vec4<T>::ProjXZ( ) const {
	return vec4<T>( x, 0, z, 0 );
}
/// Project into YZ plane
template<class T> vec4<T>	vec4<T>::ProjYZ( ) const {
	return vec4<T>( 0, y, z , 0);
}
/// Project into XYZ space
template<class T> vec4<T>	vec4<T>::ProjXYZ( ) const {
	return vec4<T>( x, y, z , 0);
}
/// get vec3
template<class T> vec3<T>	vec4<T>::xyz( ) const {
	return vec3<T>( x, y, z );
}
/// Linear interpolation
template<class T> vec4<T>	vec4<T>::Linear( const vec4<T> &vec, const T &cur, const T &begin, const T &end ) {
	if( end <= begin) return vec4<T>(0);

	vec4<T> interpolator;
	T ratio = (end - begin) / (cur - begin);
	interpolator.x = (x + vec.x)*ratio;
	interpolator.y = (y + vec.y)*ratio;
	interpolator.z = (z + vec.z)*ratio;
	interpolator.w = (w + vec.w)*ratio;

	return *this + interpolator;
}


											// ///////////////////
											// QUATERNION
											// /////////////////

/// Quaternion Constructors:

/// Quaternion from axis-angle (vec3)
template<class T> quat<T>::quat( const vec3<T> &v1, T v2 ){
	T angle	= T((v2));
	T result = T(sin(angle/(T)2));
	w = T(cos(angle/(T)2));

	// Calculate the x, y and z of the quaternion
	x = T(v1.x * result);
	y = T(v1.y * result);
	z = T(v1.z * result);
}
/// Quaternion from axis-angle (vec4)
template<class T> quat<T>::quat( const vec4<T> &v1, T ww ){
	T angle	= T((ww));
	T result = T(sin(angle/(T)2));
	w = T(cos(angle/(T)2));

	// Calculate the x, y and z of the quaternion
	x = T(v1.x * result);
	y = T(v1.y * result);
	z = T(v1.z * result);
}
/// Quaternion from eulers
template<class T> quat<T>::quat( const T &yaw, const T &pitch, const T &roll ){
	// Basically we create 3 Quaternions, one for pitch, one for yaw, one for roll
	// and multiply those together.
	// the calculation below does the same, just shorter

	T cr, cp, cy, sr, sp, sy, cpcy, spsy;
	// calculate trig identities
	cr = cos((roll/2));
	cp = cos((pitch/2));
	cy = cos((yaw/2));

	sr = sin((roll/2));
	sp = sin((pitch/2));
	sy = sin((yaw/2));

	cpcy = cp * cy;
	spsy = sp * sy;

	w = cr * cpcy + sr * spsy;
	x = sr * cpcy - cr * spsy;
	y = cr * sp * cy + sr * cp * sy;
	z = cr * cp * sy - sr * sp * cy;
}
/// Quaternion from axii and angles
template<class T> quat<T>::quat( const vec3<T> &up, const T &yaw, const vec3<T> &right, const T &pitch, const vec3<T> &look, const T &roll ){
	// Create a quaternion for each rotation, and multiply them
	// together.  We normalize them to avoid using the conjugate
	quat<T> result;
	quat<T> qyaw	= quat<T>( up,		yaw ).N();
	quat<T> qtilt	= quat<T>( right, pitch ).N();
	quat<T> qroll	= quat<T>( look,  roll ).N();
	result = qyaw * qtilt * qroll;
	x = result.x;
	y = result.y;
	z = result.z;
	w = result.w;

	N();
}

///\brief Translate axis angle to quaternion
template<class T> void 		quat<T>::FromAxisAngle( vec3<T> _kAngle ) {
	T fAngle;
	vec3<T> kSin, kCos;

	fAngle = _kAngle.x * 0.5f;
	kSin.x = sin( fAngle );
	kCos.x = cos( fAngle );
	fAngle = _kAngle.y * 0.5f;
	kSin.y = sin( fAngle );
	kCos.y = cos( fAngle );
	fAngle = _kAngle.z * 0.5f;
	kSin.z = sin( fAngle );
	kCos.z = cos( fAngle );

	T fSqrCos = kCos.x * kCos.y;
	T fSqrSin = kSin.x * kSin.y;

	x = ( T )( kSin.x * kCos.y * kCos.z - kCos.x * kSin.y * kSin.z );
	y = ( T )( kCos.x * kSin.y * kCos.z + kSin.x * kCos.y * kSin.z );
	z = ( T )( fSqrCos * kSin.z - fSqrSin * kCos.z );
	w = ( T )( fSqrCos * kCos.z + fSqrSin * kSin.z );
}
/// Self normalize
template<class T> quat<T>	quat<T>::N( ) {
	// Don't normalize if we don't have to
	T mag2 = w * w + x * x + y * y + z * z;
	if( fabs( mag2 - 1.0 ) > EPS ) {
		T mag = sqrt( mag2 );
		x /= mag;
		y /= mag;
		z /= mag;
		w /= mag;
		return *this;
	}
	return *this;
}
/// Get normalized version
template<class T> quat<T>	quat<T>::NormalizedQ( ) const {
	// Don't normalize if we don't have to
	T mag2 = w * w + x * x + y * y + z * z;
	if( fabs( mag2 - 1.0 ) > EPS ) {
		T mag = sqrt( mag2 );
		return *this / mag;
	}
	return *this;
}
/// Get conjugated version
template<class T> quat<T>	quat<T>::Conjugated( ) 	const {
	return quat<T>( -x, -y, -z,  w );
}
/// Self conjugate
template<class T> void		quat<T>::Conjugate( ) {
	Set( -x, -y, -z, w );
}
/// Quaternion * Quaternion case
template<class T> quat<T>	quat<T>::operator* ( const quat<T> &mul )	const {
	// Multiplying q1 with q2 applies the rotation q2 to q1
	quat<T> r;

	r.w = w*mul.w - x*mul.x - y*mul.y - z*mul.z;
	r.x = w*mul.x + x*mul.w + y*mul.z - z*mul.y;
	r.y = w*mul.y + y*mul.w + z*mul.x - x*mul.z;
	r.z = w*mul.z + z*mul.w + x*mul.y - y*mul.x;

	return r;
}
/// Quaternion * vec3 case
template<class T> vec3<T>	quat<T>::operator* ( const vec3<T> &mul )	const {
	// Multiplying a quaternion q with a vector v applies the q-rotation to v
	vec3<T> vn(mul);
	quat<T> vecQuat, resQuat;
	vecQuat.Set( vn.x, vn.y, vn.z, 0 );

	resQuat = *this * vecQuat * Conjugated();

	return vec3<T>( resQuat.x, resQuat.y, resQuat.z );
}
/// Quaternion *= Quaternion
template<class T> void 		quat<T>::operator*=( const quat<T> &mul ) {
	quat<T> q = *this * mul;
	Set( q.x, q.y, q.z, q.w );
}
/// Quaternion *= Scalar
template<class T> void 		quat<T>::operator*=( const T &mul ) {
	Set( x*mul, y*mul, z*mul, w*mul );
}
/// Quaternion /= Scalar
template<class T> void 		quat<T>::operator/=( const T &mul ) {
	Set( x/mul, y/mul, z/mul, w/mul );
}
/// Set values (3)
template<class T> void		quat<T>::Set( T const &xx, T const &yy, T const &zz ) {
	x = xx; y = yy; z = zz;
}
/// Set values (4)
template<class T> void		quat<T>::Set( T const &xx, T const &yy, T const &zz, T const &ww ) {
	x = xx; y = yy; z = zz; w = ww;
}
/// Quaternion / Scalar
template<class T> quat<T>	quat<T>::operator/ ( const T &mul )	const {
	return quat( x/mul, y/mul, z/mul, w/mul );
}
/// Get axis and angle around it
template<class T> void		quat<T>::GetRotation( vec3<T> &axe, T &angle ) {
	quat<T> temp = NormalizedQ();
	T cos_a = temp.w;
	angle = acos( cos_a ) * ((T)2);
	T sin_a = sqrt( (T)1.0 - cos_a * cos_a );
	if ( fabs( sin_a ) < (T)EPS ) sin_a = (T)1;
	axe.x = temp.x / sin_a;
	axe.y = temp.y / sin_a;
	axe.z = temp.z / sin_a;
}
/// Create LookAt Quaternion from a 3D direction
template<class T> void		quat<T>::LookAt( const vec3<T> &dir ) {
	vec3<T> look = (*this) * vec3<T>(FORWARD);
	vec3<T> axis = dir%look;
	T angle = ((T)(1))/(dir*look);
	*this = quat<T>(axis,angle);
}
/// Quaternion == Quaternion
template<class T> bool 		quat<T>::operator==( const quat<T> &ext ) const {
	return ( x == ext.x && y == ext.y && z == ext.z && w == ext.w );
}
/// Quaternion != Quaternion
template<class T> bool 		quat<T>::operator!=( const quat<T> &ext ) const {
	return ( x != ext.x || y != ext.y || z != ext.z || w != ext.w );
}

template class vec2<real>;
//template class vec2<float>;
//template class vec2<double>;

template class vec3<real>;
//template class vec3<float>;
//template class vec3<double>;

template class vec4<real>;
//template class vec4<float>;
//template class vec4<double>;

template class quat<real>;
//template class quat<float>;
//template class quat<double>;
